/**
 * @file test_work_item_single_shot.cpp
 * @brief Unit tests for WorkItemSingleShot
 */

#include <px4_platform_common/px4_work_queue/work_item_single_shot.hpp>
#include <px4_platform_common/px4_work_queue/work_queue_manager.hpp>
#include "FreeRTOS.h"
#include "task.h"

// Forward declaration for debug_printf
extern "C" int debug_printf(const char *format, ...);

using namespace px4;

// Test data structure
struct TestData
{
	uint32_t counter{0};
	hrt_abstime execution_time{0};
	void *received_arg{nullptr};
};

// Worker method for testing
static void test_worker(void *arg)
{
	TestData *data = (TestData *)arg;
	data->counter++;
	data->execution_time = hrt_absolute_time();
	data->received_arg = arg;
}

// Worker method that takes some time
static void slow_worker(void *arg)
{
	TestData *data = (TestData *)arg;
	vTaskDelay(pdMS_TO_TICKS(50));  // Simulate 50ms work
	data->counter++;
	data->execution_time = hrt_absolute_time();
}

// Worker method that modifies a value
static void increment_worker(void *arg)
{
	uint32_t *value = (uint32_t *)arg;
	(*value)++;
}

extern "C" void test_work_item_single_shot(void)
{
	debug_printf("\n========================================\n");
	debug_printf("  WorkItemSingleShot Tests\n");
	debug_printf("========================================\n");

	// Start WorkQueueManager if not already running
	WorkQueueManagerStart();

	uint32_t total_tests = 0;
	uint32_t passed_tests = 0;

	// Test 1: Basic construction and execution
	{
		debug_printf("Test 1: Basic construction and execution\n");

		TestData data;
		WorkItemSingleShot item(px4::wq_configurations::test1, test_worker, &data);

		total_tests++;
		if (data.counter == 0) {
			debug_printf("  ✓ Counter initialized to 0\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Counter should be 0 (counter=%lu)\n", data.counter);
		}

		hrt_abstime start_time = hrt_absolute_time();
		item.ScheduleNow();
		item.wait();  // Block until execution completes
		hrt_abstime end_time = hrt_absolute_time();

		total_tests++;
		if (data.counter == 1) {
			debug_printf("  ✓ Worker executed once (counter=%lu)\n", data.counter);
			passed_tests++;
		} else {
			debug_printf("  ✗ Worker should have executed once (counter=%lu)\n", data.counter);
		}

		total_tests++;
		if (data.received_arg == &data) {
			debug_printf("  ✓ Correct argument passed to worker\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Incorrect argument passed to worker\n");
		}

		total_tests++;
		if (data.execution_time >= start_time && data.execution_time <= end_time) {
			debug_printf("  ✓ Execution time within expected range\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Execution time out of range\n");
		}
	}

	// Test 2: Wait blocks until execution completes
	{
		debug_printf("Test 2: Wait blocks until execution completes\n");

		TestData data;
		WorkItemSingleShot item(px4::wq_configurations::test1, slow_worker, &data);

		hrt_abstime start_time = hrt_absolute_time();
		item.ScheduleNow();
		item.wait();  // Should block for ~50ms
		hrt_abstime elapsed = hrt_absolute_time() - start_time;

		total_tests++;
		if (data.counter == 1) {
			debug_printf("  ✓ Worker executed (counter=%lu)\n", data.counter);
			passed_tests++;
		} else {
			debug_printf("  ✗ Worker should have executed (counter=%lu)\n", data.counter);
		}

		total_tests++;
		if (elapsed >= 45000 && elapsed <= 70000) {  // 45-70ms tolerance
			debug_printf("  ✓ Wait blocked for correct duration (%llu us)\n", elapsed);
			passed_tests++;
		} else {
			debug_printf("  ✗ Wait duration incorrect (%llu us, expected ~50000 us)\n", elapsed);
		}
	}

	// Test 3: Multiple single-shot items
	{
		debug_printf("Test 3: Multiple single-shot items\n");

		uint32_t value1 = 0;
		uint32_t value2 = 0;
		uint32_t value3 = 0;

		WorkItemSingleShot item1(px4::wq_configurations::test1, increment_worker, &value1);
		WorkItemSingleShot item2(px4::wq_configurations::test1, increment_worker, &value2);
		WorkItemSingleShot item3(px4::wq_configurations::test1, increment_worker, &value3);

		item1.ScheduleNow();
		item2.ScheduleNow();
		item3.ScheduleNow();

		item1.wait();
		item2.wait();
		item3.wait();

		total_tests++;
		if (value1 == 1 && value2 == 1 && value3 == 1) {
			debug_printf("  ✓ All workers executed (v1=%lu, v2=%lu, v3=%lu)\n", value1, value2, value3);
			passed_tests++;
		} else {
			debug_printf("  ✗ Not all workers executed (v1=%lu, v2=%lu, v3=%lu)\n", value1, value2, value3);
		}
	}

	// Test 4: Constructor with WorkItem reference
	{
		debug_printf("Test 4: Constructor with WorkItem reference\n");

		// Create a regular work item on test2 queue
		class TestWorkItem : public WorkItem
		{
		public:
			TestWorkItem() : WorkItem("test_ref", px4::wq_configurations::test2) {}
		protected:
			void Run() override {}
		};

		TestWorkItem ref_item;
		TestData data;

		// Create single-shot item using the reference work item
		WorkItemSingleShot item(ref_item, test_worker, &data);

		item.ScheduleNow();
		item.wait();

		total_tests++;
		if (data.counter == 1) {
			debug_printf("  ✓ Worker executed on referenced work queue (counter=%lu)\n", data.counter);
			passed_tests++;
		} else {
			debug_printf("  ✗ Worker should have executed (counter=%lu)\n", data.counter);
		}
	}

	// Test 5: Different work queues
	{
		debug_printf("Test 5: Different work queues\n");

		TestData data1, data2;

		WorkItemSingleShot item1(px4::wq_configurations::test1, test_worker, &data1);
		WorkItemSingleShot item2(px4::wq_configurations::test2, test_worker, &data2);

		item1.ScheduleNow();
		item2.ScheduleNow();

		item1.wait();
		item2.wait();

		total_tests++;
		if (data1.counter == 1 && data2.counter == 1) {
			debug_printf("  ✓ Workers executed on different queues (c1=%lu, c2=%lu)\n",
				data1.counter, data2.counter);
			passed_tests++;
		} else {
			debug_printf("  ✗ Workers should have executed (c1=%lu, c2=%lu)\n",
				data1.counter, data2.counter);
		}
	}

	// Test 6: Immediate wait (schedule and wait immediately)
	{
		debug_printf("Test 6: Immediate wait (schedule and wait immediately)\n");

		TestData data;
		WorkItemSingleShot item(px4::wq_configurations::test1, test_worker, &data);

		hrt_abstime start_time = hrt_absolute_time();
		item.ScheduleNow();
		item.wait();  // Wait immediately after scheduling
		hrt_abstime elapsed = hrt_absolute_time() - start_time;

		total_tests++;
		if (data.counter == 1) {
			debug_printf("  ✓ Worker executed (counter=%lu)\n", data.counter);
			passed_tests++;
		} else {
			debug_printf("  ✗ Worker should have executed (counter=%lu)\n", data.counter);
		}

		total_tests++;
		if (elapsed < 10000) {  // Should complete quickly (< 10ms)
			debug_printf("  ✓ Wait completed quickly (%llu us)\n", elapsed);
			passed_tests++;
		} else {
			debug_printf("  ✗ Wait took too long (%llu us)\n", elapsed);
		}
	}

	// Test 7: Null argument
	{
		debug_printf("Test 7: Null argument\n");

		static bool null_arg_called = false;
		auto null_worker = [](void *arg) {
			null_arg_called = true;
			// Verify arg is NULL
			if (arg == nullptr) {
				debug_printf("    Worker received NULL argument as expected\n");
			}
		};

		null_arg_called = false;
		WorkItemSingleShot item(px4::wq_configurations::test1, null_worker, nullptr);

		item.ScheduleNow();
		item.wait();

		total_tests++;
		if (null_arg_called) {
			debug_printf("  ✓ Worker executed with NULL argument\n");
			passed_tests++;
		} else {
			debug_printf("  ✗ Worker should have executed\n");
		}
	}

	debug_printf("========================================\n");
	debug_printf("  Test Summary\n");
	debug_printf("========================================\n");
	debug_printf("Total tests: %lu\n", total_tests);
	debug_printf("Passed: %lu\n", passed_tests);
	debug_printf("Failed: %lu\n", total_tests - passed_tests);
	debug_printf("\n");

	if (passed_tests == total_tests) {
		debug_printf("✅ All WorkItemSingleShot tests PASSED!\n");
	} else {
		debug_printf("❌ Some WorkItemSingleShot tests FAILED!\n");
	}
	debug_printf("========================================\n\n");
}

